package edu.wpi.first.wpilibj.swarm;

public class TransStab {
	private MovePD    xpd;
	private MovePD    ypd;
	private RobotMain robomain;
	private boolean   needreset;
	
	public TransStab(RobotMain robotmain) {
		robomain  = robotmain;
		xpd       = new MovePD();
		ypd       = new MovePD();
		needreset = false;
	}
	
	void reset() {
		needreset = true;
	}
	
	void control(Vector transerror, Vector vel, Vector fieldoriented_out,
	             Vector mech_out) {
		if (robomain.isDisabled()) {
			needreset = true;
		}
		if (needreset) {
			transerror.setX(0.0);
			transerror.setY(0.0);
			needreset = false;
		}
		
		mech_out.setX(xpd.control(transerror.getX(), vel.getX(),
		                          fieldoriented_out.getX()));
		transerror.setX(xpd.getErr());
		
		mech_out.setY(ypd.control(transerror.getY(), vel.getY(),
		                          fieldoriented_out.getY()));
		transerror.setY(ypd.getErr());
	}
}